import math
from math import pi

import numpy as np
from pyquaternion import Quaternion
from sgp4.api import jday

# G - Newtonian constant of gravitation 	6.67384e-11 m^3 kg^-1 s^-2
# M_earth 5.97216787e+24 kg Earth mass

MACHINE_ZERO = 2 * np.finfo(np.float32).eps  # to use in comparisons

MISSION_PREFIX = "SixU"
RESULTS_FOLDER = "results"
# TODO убрать вообще длительности фаз?
# считать до окончания топлива?
MANEUVERING_MAX_DURATION = 1e5
ALPHA_GOAL = 2 * pi


################################################################################
#                             CAN bus
################################################################################
SIMULATOR_ID = 0xC0FFEE
OBC_ID = 0xFACADE

################################################################################
#                             Mass budget
################################################################################

MASS_PAYLOAD = 1.6  # https://dragonflyaerospace.com/chameleon/

MASS_PLATFORM_WITHOUT_PROPULSION = 6
# https://gomspace.com/6u-professional.aspx

M_FUEL_0 = 0.25  # water-steam initial mass, kg
MASS_PROPULSION_STRUCTURE = 0.5  # propulsion without fuel, kg

# 6U CubeSat mass without fuel, kg
SAT_DRY_MASS = (
    MASS_PAYLOAD + MASS_PLATFORM_WITHOUT_PROPULSION + MASS_PROPULSION_STRUCTURE
)

# satellite mass inside launch container, kg
SAT_START_MASS = SAT_DRY_MASS + M_FUEL_0


################################################################################
#                             Ballistic parameters
################################################################################

YEAR_START = 2020
MONTH_START = 6
DAY_START = 7
HOUR_START = 12
MINUTE_START = 0
SECOND_START = 0
JD_START, JD_FR_START = jday(
    YEAR_START, MONTH_START, DAY_START, HOUR_START, MINUTE_START, SECOND_START
)

# TODO change to 600 km SSO orbit satellite
# Define TLE (ISS)
# https://www.celestrak.com/NORAD/elements/stations.txt
# accessed 2020-06-07 14:41 MSK (GMT+3)
# ISS (ZARYA)
TLE_LINE_1 = "1 25544U 98067A   20159.17750479 -.00000282  00000-0  30238-5 0  9991"
TLE_LINE_2 = "2 25544  51.6459  38.0125 0002438  28.0771 113.9243 15.49425665230468"

MIDDLE_AREA = 0.2 * 0.3  # middle area for cubesat 6U (flat plate), m^2
C_F = 1.95  # drag coefficient for flat plate


################################################################################
#                              ADCS parameters
################################################################################

# собственная матрица датчика угловой скорости (angular velocity sensor matrix)
AVS_MATRIX = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
# .radians() converts value from degrees to radians
ANGLE_ACCURACY = math.radians(10)  # Orientation accuracy, rad
OMEGA_ACCURACY = math.radians(1)  # Detumbling accuracy, rad/s

# Initial conditions from ARAPAIMA mission
# Attitude Determination & Control System Design
# and Implementation for a 6U CubeSat Proximity Operations Mission
J_SAT = np.array(
    [
        [0.0372, -0.0007, -0.0001],
        [-0.0007, 0.0703, -0.0006],
        [-0.001, -0.0006, 0.0922],
    ]
)  # kg m^2
# eq. 26 in 3CAT 2 Attitude Determination and Control System for a GNSS R
# Earth Observation 6U CubeSat Mission

# Коэффициенты обратной связи ПД регулятора СОС для различных режимов работы КА
K_P = {
    "eclipse": 0.0 * J_SAT,  # no difference since q_cmd = q_bi
    "out_of_fuel": 0.0 * J_SAT,
    "heat_accumulation": 0.1 * J_SAT,
    "orbital_orientation": 0.1 * J_SAT,
    "thrusting": 0.1 * J_SAT,
    "waiting_thrusting_allowed_zone": 0.1 * J_SAT,
}

K_D = {
    "eclipse": 0.5 * J_SAT,
    "out_of_fuel": 0.5 * J_SAT,
    "heat_accumulation": 0.5 * J_SAT,
    "orbital_orientation": 0.5 * J_SAT,
    "thrusting": 0.5 * J_SAT,
    "waiting_thrusting_allowed_zone": 0.5 * J_SAT,
}

SOL_GOAL_B = [0, 0, 1]  # Goal Solar vector in body frame
LVLH_O1_GOAL_B_ACCELERATING = [-1, 0, 0]  # Goal O1 LVLH vector in body frame

WHEEL_MAX_RATE = 6500 / 60  # sec^-1
WHEEL_INERTIA = 20e-3 / WHEEL_MAX_RATE  # kg*m2
WHEEL_MAX_ACCELERATION = 3.2e-3 / WHEEL_INERTIA  # sec^-2

WHEEL_ANGLE = math.acos(5.9 / (2 * 3.2))
# https://nanoavionics.com/cubesat-components/cubesat-reaction-wheels-control-system-satbus-4rw/

WHEEL_INI_RATE = [0, 0, 0, 0]

QUATERNION_0 = Quaternion([0.4, -0.2, -0.4, 0.8]).normalised
OMEGA_0 = [1.5e-3, -1e-3, -0.5e-3]  # from real mission
# OMEGA_0 = [math.radians(10), math.radians(10), math.radians(10)]  # for debug

# THRUST_OFFSET = np.array([0, 1e-4, 1e-4])
THRUST_OFFSET = np.array([0, 0, 0])
THRUST_UNIT_V = np.array([-1, 0, 0])

# Magnetic sensor mounting matrix
MAGNET_MOUNT = np.array(
    [
        [0, 0, 1],
        [-0.866025, -0.00872654, 0],
        [0.5, 0.999962, 0],
    ]
)

# magnetorquers (B-dot algorithm)
# http://www.ece3sat.com/blog/2018-01-25-how-to-slow-down-rotation-the-bdot-algorithm/
# Coefficient K (page 10) https://www.keldysh.ru/papers/2018/prep2018_7.pdf
B_DOT_K = 1.25

# макс. моменты катушек
COILS_MAT = np.array(
    [
        [1, 0, 0],
        [0, 1, 0],
        [0, 0, 1],
    ]
)

# параметр, характеризующий, во сколько раз полезная составляющая
# момента должна превышать сумму бесполезной и вредной для включения катушки
SWITCHING_COEF = 2


################################################################################
#                          Power budget parameters
################################################################################

POWER_CONSUMPTION = {
    "eclipse": 17,
    "heat_accumulation": 17,
    "orbital_orientation": 17,
    "out_of_fuel": 17,
    "waiting_thrusting_allowed_zone": 17,
    "thrusting": 20,
}

# BATTERY_CAPACITY = 20 * 3600  # W*sec
BATTERY_CAPACITY = 77 * 3600  # W*sec

# https://gomspace.com/shop/subsystems/power/nanopower-bpx.aspx
# TODO BATTERY_DOD never used, add simulation stop event
# TODO BATTERY_DOD never used, add DOD level on plots
# TODO consider batteries efficiency coefficient
BATTERY_DOD = 0.8

one_unit_max_power = (2400 + 2270) / 2 * 1e-3
# FIXME change to actual solar panels configuration
SOLAR_PANELS_MAX_POWER = [
    [0, one_unit_max_power * (2 * 6 * (math.sqrt(2) / 2))],
    [0, 0],
    [0, one_unit_max_power * (2 * 6 * (math.sqrt(2) / 2) + 3)],
]
# x minus, x plus
# y minus, y plus
# z minus, z plus
# https://gomspace.com/UserFiles/Subsystems/datasheet/gs-ds-nanopower-p110-210.pdf


################################################################################
#                          Propulsion parameters
################################################################################

# Минимальная проекция орта на Солнце на ось концентратора, при которой сфокусированный
# световой поток попадает на приёмник излучения. Если проекция меньше, т.е. угол
# отклонения больше, то световой поток попадает мимо приёмника излучения и рабочее тело
# не нагревается.
CONCENTRATOR_MIN_PROJECTION = 0.98

# 4 by 4 cm opened space, other is covered in MLI
EQUIVALENT_SQUARE = 0.04 * 0.04

V_SUM = 0.5 / 1000  # tank volume converted from litres, m**3
TANK_STRUCTURE_MASS = 0.3  # tank mass that will be heated with fuel, kg
F_KR_0 = pi * (0.003**2) / 4  # nozzle critical cross section area, m^2
FE_NOZZLE_0 = pi * (0.025**2) / 4  # nozzle exit section area, m^2

T_INI_K = 300  # Initial temperature, Kelvin
VALVE_OPENING_DURATION = 1.5  # seconds
# T_INI_K = 460  # Initial temperature, Kelvin

# TODO final fuel mass back. Find why 0.01 is not working
# Расчёт завис с такими параметрами:
# t_span=[371019.129638674, 448731.4266583901]
# sat_state='eclipse'
# Sun visibility: 0.5000000000000336
# Temperature: 416.24458176553037, K
# Fuel mass: 0.04785235737409452, kg
# FINAL_FUEL_MASS = 0.01  # fuel mass that will remain, kg
FINAL_FUEL_MASS = 0.1  # fuel mass that will remain, kg

CONCENTRATOR_SIZE_X = 0.1  # m
CONCENTRATOR_SIZE_Y = 0.3  # m
CONCENTRATOR_EFFICIENCY = 0.75
